/*
 * automatic.cpp
 *
 *  Created on: Oct 21, 2012
 *      Author: Matthijs
 */
#include <automatic/automatic.h>


Automatic::Automatic () :
	behaviors()
{
	ROS_INFO("Automatic controller node launching.");
	ros::NodeHandle n;
	move_publisher 		= n.advertise<driver::Move>("motor_controller/directions", 1);
	phase_subscriber 	= n.subscribe("SLAM/phase", 1, & Automatic::switchPhase, this);

	// Start new behavior nodes.
	behaviors.push_back(new Wallfollow(n));
	behaviors.push_back(new ObstacleAvoidance(n));
	behaviors.push_back(new Exploration(n));
	behaviors.push_back(new Navigation(n));

	// Start with exploration and navigation inactive.
	behaviors[2] -> setActive(false);
	behaviors[3] -> setActive(false);

	ros::Rate loop_rate(2);
	while (ros::ok())
	{
		ros::spinOnce();
		Behavior * selected = selectBehavior();

		selected -> act(move_publisher);
		loop_rate.sleep();
	}
	ROS_INFO("Terminating automatic controller node.");
}



/**
 * 	Automatic Control
 */
int main (int argc, char **argv)
{
  	ros::init(argc, argv, "controller_automatic");
	new Automatic();
	return 0;
}
